/*
 *  TrackAgent.cpp
 *  powaah
 *
 *  Created by Tim Uusitalo on 4/19/11.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */

#include "TrackAgent.h"

#include "MathUtility.h"

#include "TrackInfo.h"

using namespace MathUtility;
using namespace TrackInfo;

TrackAgent::TrackAgent()
{
	
	for (int i = 0; i < 5; i++)
	{
		angles[i] = -90 + i*15;
		angles[18-i] = 90 - i*15;
	}
	
	for (int i = 5; i < 9; i++)
	{
		angles[i] = -20+(i-5)*5;
		angles[18-i] = 20-(i-5)*5;
	}
	
	angles[9] = 0;
	
}

TrackAgent::~TrackAgent()
{
	
}

void TrackAgent::update(CarState& cs, const float dt, TrackLearner& track)
{
	
	
	if (cs.getTrackPos() > 0)
	{
		right = track.getWidth() * 0.5f + track.getWidth() * 0.5f * (cs.getTrackPos());
	
		left = fabsf(track.getWidth() - right);
		//std::cout << "first left: " << left << "right: " << right << std::endl;

	}
	else if (cs.getTrackPos() < 0)
	{
		
		
		right = fabsf(track.getWidth() * 0.5f + track.getWidth() * 0.5f * (cs.getTrackPos() - 1.0f));
		
		left = track.getWidth() - right;
		//std::cout << "sec  left: " << left << "right: " << right << std::endl;

		
	}
	else if (cs.getTrackPos() == 0)
	{
		right = track.getWidth() * 0.5f * cs.getTrackPos();
		
		left = track.getWidth() * 0.5f * cs.getTrackPos();
	}
	
	if (cs.getTrackPos() > 1.0f || left < 0)
	{
		left = 0;
	}
	
	else if (cs.getTrackPos() < -1.0f || right < 0)
	{
		right = 0;
	}
	
	
	update(cs,dt);
}
void TrackAgent::update(CarState& cs, const float dt)
{
	current = cs;
	
	Charge rightCharge, leftCharge, frontCharge;
	
	clear();
	
	//if (cs.getTrackPos() <= -1.0f || cs.getTrackPos() >= 1.0f)
	//	return;
	
	
	for (int i = 0; i < 19; i++)
	{
		
		Charge charge;
		charge.setLength(cs.getTrack(i));
		charge.setAngle(convertDegToRad(angles[i]));
		charge.update();
		
		insertCharge(charge, field.size());
		
	}
	
	
	/*
	Vector2 pos(0, (right));
	
	rightCharge.setAngle(pos.angle() + cs.getAngle());
	rightCharge.setLength(pos.length());
	rightCharge.update();
	
	pos = Vector2(0, (-1*left));
	leftCharge.setLength(pos.length());
	leftCharge.setAngle(pos.angle() + cs.getAngle());
	leftCharge.update();
	
	insertCharge(rightCharge, 0);
	insertCharge(leftCharge, 1);
	*/
}



const float TrackAgent::getDistToLeft() const
{
	return left;
}
const float TrackAgent::getDistToRight() const
{
	return right;
}

void TrackAgent::calculatePotentialForOptions(DriverOption* const options, unsigned int size)
{
	for (int i = 0; i < size; i++)
	{
		for (int j = 0; j < field.size(); j++)
		{
		//Charge leftCharge = field[1], rightCharge = field[0];
		
		//float leftLength = 0.0f, rightLength = 0.0f;
			
			float length = (options[i].getPosition() - field[j].getPosition()).length();
			
			options[i].addPotential(-5.f / sqrtf(length));
		}
		
	}
}
